#include <assert.h>
#include "dijkstra.hh"

namespace blue{
    const int AlgDijkstra::INF = INT_MAX;

    AlgDijkstra::AlgDijkstra(std::vector<std::vector<int> > &map){
        assert(map.size()==map[0].size());
        map_ = map;
        node_max_ = map.size();
    }

    // Function that implements Dijkstra's single source shortest path algorithm 
    // for a graph represented using adjacency matrix representation 
    std::vector<int> AlgDijkstra::plan(int start_node, int end_node){

        std::vector<int> dist;   // The output array.  dist[i] will hold the shortest 
                                 // distance from src to i
    
        std::vector<bool> sptSet;// sptSet[i] will be true if vertex i is included in shortest 
                                 // path tree or shortest distance from src to i is finalized 
    
        // Initialize all distances as INFINITE and stpSet[] as false 
        pre_.clear();
        for (int i = 0; i < node_max_; i++){
            dist.push_back(INF);
            sptSet.push_back(false);
            pre_.push_back(-1);
        }

        // Distance of source vertex from itself is always 0 
        dist[start_node] = 0;

        // Find shortest path for all vertices 
        for (int count = 0; count < node_max_-1; count++) 
        { 
            // Pick the minimum distance vertex from the set of vertices not 
            // yet processed. u is always equal to src in the first iteration. 
            int u = minDistance(dist, sptSet); 
        
            // Mark the picked vertex as processed 
            sptSet[u] = true; 
        
            // Update dist value of the adjacent vertices of the picked vertex. 
            for (int v = 0; v < node_max_; v++)
                // Update dist[v] only if is not in sptSet, there is an edge from  
                // u to v, and total weight of path from src to  v through u is  
                // smaller than current value of dist[v] 
                if (!sptSet[v] && map_[u][v]!=INF && dist[u] != INF
                                            && dist[u]+map_[u][v] < dist[v]){
                    dist[v] = dist[u] + map_[u][v];
                    pre_[v] = u;
                }
        }

        return pre_;
    }

    int AlgDijkstra::minDistance(std::vector<int> dist, std::vector<bool> sptSet){
        int min = -1, val=INF;
        for(int i=0; i<dist.size(); i++){
            if(!sptSet[i] && dist[i]<val){
                min = i;
                val = dist[i];
            }
        }
        return min;
    }
}